/* 
 * File:   Track.cpp
 * Author: dave
 * 
 * Created on 2010年11月2日, 下午5:03
 */
#include "Track.h"

extern int numFrame;
extern int hdims;
extern int minT;


Track::Track(Camera *camera) {
    this->cam = camera;
}

Track::Track(const Track& orig) {
}

Track::~Track() {
}

void Track::Init()
{
    V.clear();
    id = 0;
}

bool Track::Match(CvConnectedComp comp)
{
    for(int i = 0; i < V.size(); ++i)
    {
        if(comp.rect.x > V[i].comp.rect.x + 10 || V[i].comp.rect.x > comp.rect.x + 10)
            continue;
        if(comp.rect.y > V[i].comp.rect.y + 10 || V[i].comp.rect.y > comp.rect.y + 10)
            continue;
        if(comp.area > V[i].comp.area * 2 || V[i].comp.area > comp.area * 2)
        if(comp.rect.height * comp.rect.width > V[i].comp.rect.height * V[i].comp.rect.width * 2
            || V[i].comp.rect.height * V[i].comp.rect.width > comp.rect.height * comp.rect.width * 2)
            continue;
        return true;
    }
    return false;
}

bool Track::Match(CvRect a, CvRect b)
{
    if(a.x > b.x + 10 || b.x > a.x + 10)
        return false;
    if(a.y > b.y + 10 || b.y > a.y + 10)
        return false;
    if(a.height * a.width > b.height * b.width * 2
            || b.height * b.width > a.height * a.width * 2)
        return false;
    return true;
}

bool Track::Match(trackObject obj)
{
    int x = (obj.pt0.x + obj.pt1.x) / 2, y = (obj.pt0.y + obj.pt1.y) / 2;
    for(int i = 0; i < V.size(); ++i)
    {
        if(V[i].matched) continue;
        if(x < V[i].pt0.x || x > V[i].pt1.x || y < V[i].pt0.y || y > V[i].pt1.y)
            continue;
        int X = (V[i].pt0.x + V[i].pt1.x) / 2, Y = (V[i].pt0.y + V[i].pt1.y) / 2;
        if(X < obj.pt0.x || X > obj.pt1.x || Y < obj.pt0.y || Y > obj.pt1.y)
            continue;
        obj.id = V[i].id;
        V[i] = obj;
        V[i].matched = true;
        return true;
    }
    return false;
}

void Track::NewObject(vector<trackObject> obj)
{
    IplImage *image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
    IplImage *mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);

    cvCopy(cam->pPre, image, 0);
    cvCvtColor(image, cam->hsv, CV_BGR2HSV);
    cvInRangeS(cam->hsv, cvScalar(0, 30, 10, 0),
            cvScalar(180, 256, 256,0), mask );
    cvSplit(cam->hsv, cam->hue, 0, 0, 0);

    for(int i = 0; i < obj.size(); ++i)
    {
        //搜索边框
        window = cvRect(obj[i].pt0.x, obj[i].pt0.y,
                obj[i].pt1.x - obj[i].pt0.x + 1, obj[i].pt1.y - obj[i].pt0.y + 1);
        //设置图像的感兴趣区域
        cvSetImageROI(cam->hue, window);
        cvSetImageROI(mask, window);
        cvCalcHist(&cam->hue, cam->hist, 0, mask);
        float max_val = 0.0;
        cvGetMinMaxHistValue(cam->hist, 0, &max_val, 0, 0 );
        cvConvertScale(cam->hist->bins, cam->hist->bins,
                max_val ? 255. / max_val : 0, 0 );
        cvResetImageROI(cam->hue);
        cvResetImageROI(mask);

        //计算目标直方图的反向投影
        try {
            cvCalcBackProject(&(cam->hue), cam->backProject, cam->hist);
        } catch(...) {
            
        }

        //蒙板
        cvAnd(cam->backProject, mask, cam->backProject, 0);
        //执行CamShift算法跟踪目标
        cvCamShift(cam->backProject, window,
                cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
                &comp, &box);

        if(!Match(comp))
        {
            trackObject tmpObj = obj[i];
            tmpObj.id = id++;
            tmpObj.lostfrm = 0;
            tmpObj.comp.rect = window;
            tmpObj.comp.area = window.height * window.width;
            //保存跟踪结果
            V.push_back(tmpObj);
        }
    }
    cvReleaseImage(&image);
    cvReleaseImage(&mask);
}

void Track::TrackNew(vector<trackObject> obj)
{
    for(int i = 0; i < obj.size(); ++i)
    {
        if(!Match(obj[i]))
        {
            obj[i].comp.rect = cvRect(obj[i].pt0.x, obj[i].pt0.y,
                    obj[i].pt1.x - obj[i].pt0.x + 1, obj[i].pt1.y - obj[i].pt0.y + 1);
            obj[i].id = id++;
            obj[i].lostfrm = 0;
            obj[i].matched = true;
            obj[i].comp.area = obj[i].comp.rect.width * obj[i].comp.rect.height;
            //保存跟踪结果
            V.push_back(obj[i]);
        }
    }
}

void Track::TrackExist()
{
    IplImage *image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
    IplImage *mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);

    cvCopy(cam->pFrame, image, 0);
    cvCvtColor(image, cam->hsv, CV_BGR2HSV);
    cvInRangeS(cam->hsv, cvScalar(0, 30, 10, 0),
            cvScalar(180, 256, 256,0), mask );
    cvSplit(cam->hsv, cam->hue, 0, 0, 0);

    for(int i = 0; i < V.size(); ++i)
    {
        if(V[i].matched)
        {
            V[i].matched = false;
            continue;
        }
        //设置图像的感兴趣区域
        cvSetImageROI(cam->hue, V[i].comp.rect);
        cvSetImageROI(mask, V[i].comp.rect);
        cvCalcHist(&cam->hue, cam->hist, 0, mask);
        float max_val = 0.0;
        cvGetMinMaxHistValue(cam->hist, 0, &max_val, 0, 0 );
        cvConvertScale(cam->hist->bins, cam->hist->bins,
                max_val ? 255. / max_val : 0, 0 );
        cvResetImageROI(cam->hue);
        cvResetImageROI(mask);
        //计算目标直方图的反向投影
        try{
            cvCalcBackProject(&(cam->hue), cam->backProject, cam->hist);
        } catch(...) {

        }
        //蒙板
        cvAnd(cam->backProject, mask, cam->backProject, 0);
        //执行CamShift算法跟踪目标
        cvCamShift(cam->backProject, V[i].comp.rect,
                cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
                &comp, &box);

        if(Match(comp.rect, V[i].comp.rect))
        {
            V[i].pt0 = cvPoint(comp.rect.x, comp.rect.y);
            V[i].pt1 = cvPoint(comp.rect.x + comp.rect.width - 1,
                    comp.rect.y + comp.rect.height - 1);
            V[i].comp = comp;
            V[i].lostfrm = 0;
        } else {
            V[i].lostfrm++;
        }
        V[i].matched = false;
    }
    vector<trackObject>::iterator iter = V.end();
    vector<trackObject>::iterator tmp;
    if(V.size() != 0){
        bool f = false;
        while(true)
        {
            if(iter == V.begin()) f = true;
            int p = V.size();
            if(iter->lostfrm > 10)
            {
                tmp = iter;
                V.erase(tmp);
            }
            if(f) break;
            iter--;
        }
    }
    cvReleaseImage(&image);
    cvReleaseImage(&mask);
}

void Track::CamShift(vector<trackObject> obj)
{
    //NewObject(obj);
    TrackNew(obj);

    TrackExist();

    printf("TotalObj\t%d\n", V.size());
}

void Track::Draw(IplImage * image, CvScalar color)
{
    char s[8];
    CvFont * font;
    cvInitFont(font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0);
    for(int i = 0; i < V.size(); ++i)
    {
        if(V[i].lostfrm) continue;
        cvRectangle(image, V[i].pt0, V[i].pt1, color, 2);
        sprintf(s, "%d", V[i].id);
        cvPutText(image, s, V[i].pt1, font, color);
    }
}